#include <Wire.h> 
#include <LiquidCrystal_I2C.h>

//Expander
const int exAddr = 0x20;
#define ex_1 100
#define ex_2 101
#define ex_3 102
#define ex_4 103
#define ex_5 104
#define ex_6 105
#define ex_7 106
#define ex_8 107

#define m1_a 9
#define m1_b 8
#define m1_e 10
#define m2_e 11
#define m2_a 7
#define m2_b 6
//Sensoren
#define ir_re   ex_1
#define ir_mire ex_2
#define ir_mili ex_3
#define ir_li   ex_4
#define ir_hr   4
#define ir_hl   5

const int turnSpeed = 80;


LiquidCrystal_I2C lcd(0x27,16,2);

void setup() {
  for(int i=2;i<14;i++)
  {
    pinMode(i,OUTPUT);
    digitalWrite(i,LOW);
  }
    
  //Serial.begin(9600);
  
  lcd.init();    // initialize the lcd 
  lcd.backlight();  
  lcd.home();
  lcd.print("Walli 2.0");
  lcd.setCursor(0,1);
  lcd.print("      setup...");
  
  /*pinMode(ir_re, INPUT);
  pinMode(ir_mire, INPUT);
  pinMode(ir_mili, INPUT);
  pinMode(ir_li, INPUT);*/
  pinMode(ir_hr, INPUT);
  pinMode(ir_hl, INPUT);
    
  randomSeed(analogRead(3));
  
  while(!ExpanderAvailable()){
    lcd.setCursor(0,1);lcd.print("expander not found!");
  }
  
  //setupOLED();
  lcd.setCursor(0,1);lcd.print("      ready...");
  
  blink();
  lcd.setCursor(0,0);
  lcd.print("                ");
  lcd.setCursor(0,1);
  lcd.print("                ");
  forw();
	
}

int whilePin=0;
int trapdet           = 0;
unsigned long timebuf = 0;

const byte dir_left = 1;
const byte dir_right = 2;
const byte dir_forward = 3;
const byte dir_backward = 4;
 
void loop() {
  int a=analogRead(0);
  a=a/4; //von 1024 auf 256
  if(!checkWhilePin())
    setSpeed(a);
  
  //Richtung solange beibehlaten bis der enstpr. Sensor frei wird
  if(checkWhilePin() && get(whilePin)){
	whilePin = 0;
    stop();
	forw();
  }
  
  testSensor(ir_re, dir_left);
  testSensor(ir_li, dir_right);
   
  int dir = dir_left;
  if(random(9)>5) dir = dir_right;
  
  testSensor(ir_mili, dir); 
  testSensor(ir_mire, dir); 
  
  lcdWhilePin();  
  trapdetect();
  
  delay(1);
}

boolean checkWhilePin(){return (whilePin>0);}

boolean testSensor(int aSens, byte aDirection){
  if(!checkWhilePin() && !get(aSens)){
    stop(); 
    switch (aDirection) {
      case dir_left:
        left();
        break;
      case dir_right:
        right();
        break;
    }
	  whilePin=aSens;
    setSpeed(turnSpeed);
  }
  return checkWhilePin();
}

void lcdWhilePin(){
  lcd.setCursor(0,0);
  switch (whilePin) {
    case ir_re:
	  lcd.print(F(" ri "));
	  break;
    case ir_li:
	  lcd.print(F(" le "));
	  break;
	case ir_mili:
	  lcd.print(F("mile"));
	  break;
	case dir_left:
	  lcd.print(F("miri"));
	  break;
	default:
	  lcd.print(F("    "));
  }    
}

void setSpeed(byte Speed){
  if(Speed<40){
    stop();
    digitalWrite(m1_e,LOW);
    digitalWrite(m2_e,LOW);
  }
  else {
    analogWrite(m1_e,Speed);
    analogWrite(m2_e,Speed); 
  }
  lcd.setCursor(10,1);
  if(Speed<100) 
    lcd.print(' '); 	
  lcd.print(Speed);  
}

void lcdDir(byte aDirection){
  lcd.setCursor(0,1);
  String dirStr = "unknown";
  switch (aDirection) {
    case dir_forward:
	  dirStr = "forward ";
	  break;
    case dir_backward:
	  dirStr = "backward";
	  break;
	case dir_right:
	  dirStr = "right   ";
	  break;
	case dir_left:
	  dirStr = "left    ";
	  break;
  }
  lcd.print(dirStr);
}

void trapdetect(){
  if(millis()-timebuf > 5000){
    
    if(trapdet>5){
      lcd.setCursor(0,0);
	  lcd.print(F("trap"));
	  
      stop();
	  setSpeed(turnSpeed);
      bacw();
      delay(1000);
      stop();
	  
      int randNum = random(9);
      
      if(randNum>5){
        right();
        delay(500);
        while(!get(ir_li) || !get(ir_mili) || !get(ir_mire) || !get(ir_re)){
          right();
        }
      }
      else  { 
        left();
        delay(500);
        while(!get(ir_li) || !get(ir_mili) || !get(ir_mire) || !get(ir_re)){
          left();
        }
      }
      stop();
    }
   
    timebuf=millis();
    trapdet=0;
  }    
}

 void forw(){
  set(m1_a, HIGH);
  set(m1_b, LOW);
  set(m2_a, HIGH);
  set(m2_b, LOW);
  lcdDir(dir_forward);
}

void bacw(){
  set(m1_a, LOW);
  set(m1_b, HIGH);
  set(m2_a, LOW);
  set(m2_b, HIGH); 
  lcdDir(dir_backward);  
}

void left()
{
  set(m1_a, LOW);
  set(m1_b, HIGH);  
  set(m2_a, HIGH);
  set(m2_b, LOW);
  lcdDir(dir_left);
}

void right()
{
  set(m2_a, LOW);
  set(m2_b, HIGH);
  set(m1_a, HIGH);
  set(m1_b, LOW);
  lcdDir(dir_right);
}
 
void set(int pin, boolean wert) {
  digitalWrite(pin, wert);
}

boolean get(int pin) {
  if (pin<100) {
    return digitalRead(pin);
  }
  else {
    byte data = expanderRead();
    if (data==-1){
      lcd.setCursor(10,0);lcd.print("ex err");
    } 
    return ((data > -1) && bitRead(data, pin/100));
  }
}
  
void stop(){
  trapdet++;
  set(m1_a, LOW);
  set(m1_b, LOW);
  set(m2_a, LOW);
  set(m2_b, LOW);
  delay(300);
}
  
void blink(){
  pinMode(13, OUTPUT);
  blink1(HIGH);
  blink1(LOW);
  blink1(HIGH);
  blink1(LOW);
  blink1(HIGH);
  blink1(LOW);
  blink1(HIGH);
  blink1(LOW);
  blink1(HIGH);
  blink1(LOW);
 }
void blink1(boolean pp){
    set(13, pp);
    delay(400);
}
 
byte expanderRead() {
  int _data = -1;
  Wire.requestFrom(exAddr, 1); //Read 1byte from expander
  if(Wire.available()) {
    _data = Wire.read();
  }
  return _data;
}

byte ExpanderAvailable() {
  Wire.beginTransmission(exAddr);
  return (Wire.endTransmission()==0);
}
